﻿using TPXSDK.NET.Share;

namespace TPXSDK.NET.GIS;

/// <summary>
/// 不同转换算法有不同的精度和适用范围，以及计算量，选择时需根据具体需求权衡。
/// ======================================================================================
///         |平均迭代次数	|每次迭代计算量	        | 总计算复杂度  | 适用场景
/// Bowring	| 2-4次	    |2三角函数+1平方根         | 最低        | 实时应用、嵌入式系统
/// Ferrari | 3-6次	    |3三角函数+1平方根         | 中等        | 一般高精度应用
/// Vincenty| 5-10次	    |3三角函数+1平方根+额外计算 | 最高        | 科学研究、毫米级精度
/// ======================================================================================
/// </summary>
public class CoordTools
{
    private const double WGS84_A = 6378137.0;// 基准椭球体的长半径a 
    private const double WGS84_F = 1 / 298.257223565;// 基准椭球体的极扁率f 
    private const double WGS84_B = (1 - WGS84_F) * WGS84_A;// 基准椭球体的长半径b 
    private const double EPSILON = 0.000000000000001;// 误差 

    /**
     * @brief LLA（WGS84）转ECEF
     * @param lat   纬度，单位度，范围-90~90.
     * @param lon   经度，单位度，范围-180~180.
     * @param alt   高度，单位米.
     * @param x     坐标x，单位米.
     * @param y     坐标y，单位米.
     * @param z     坐标z，单位米.
     */
    static public double[] LLA2ECEF(double lat, double lon, double alt)
    {
        double WGS84_E2 = WGS84_F * (2 - WGS84_F);
        double latRad = DataUtils.Deg2Rad(lat);
        double lonRad = DataUtils.Deg2Rad(lon);
        double N = WGS84_A / Math.Sqrt(1 - WGS84_E2 * Math.Sin(latRad) * Math.Sin(latRad));

        var x = (N + alt) * Math.Cos(latRad) * Math.Cos(lonRad);
        var y = (N + alt) * Math.Cos(latRad) * Math.Sin(lonRad);
        var z = (N * (1 - WGS84_E2) + alt) * Math.Sin(latRad);

        return [x, y, z];
    }

    /**
      * @brief ECEF转LLA（WGS84）
      * @param x     坐标x，单位米.
      * @param y     坐标y，单位米.
      * @param z     坐标z，单位米.
      * @param lat   纬度，单位度，范围-90~90.
      * @param lon   经度，单位度，范围-180~180.
      * @param alt   高度，单位米.
    */
    static public double[] ECEF2LLA(double x, double y, double z)
    {
        double WGS84_E = Math.Sqrt(WGS84_A * WGS84_A - WGS84_B * WGS84_B) / WGS84_A;

        double p = Math.Sqrt(x * x + y * y);
        double N = 0.0;
        double curLat = 0.0;
        double calLat = Math.Atan2(z, p);
        for (int i = 0; DataUtils.Rad2Deg(Math.Abs(calLat - curLat)) > EPSILON && i < 25; i++)
        {
            curLat = calLat;
            N = WGS84_A / Math.Sqrt(1 - WGS84_E * WGS84_E * Math.Sin(curLat) * Math.Sin(curLat));
            calLat = Math.Atan2(z + N * WGS84_E * WGS84_E * Math.Sin(curLat), p);
        }

        var lat = DataUtils.Rad2Deg(Math.Atan2(y, x));
        var lon = DataUtils.Rad2Deg(calLat);
        var alt = z / Math.Sin(calLat) - N * (1 - WGS84_E * WGS84_E);

        return [lon, lat, alt];
    }
}
